#ifndef MY_CONTROLLER_LUNTUI_H
#define MY_CONTROLLER_LUNTUI_H

// float Poly_Coefficient[8][4] = {{3.515e-12, -7.2537e-13, 4.388e-14, -0.010351},
// 								{26.7451, -7.9462, 0.83722, -0.087871},
// 								{-65.1454, 23.0838, -3.4635, -0.6282},
// 								{4.0412, -1.3881, 0.028802, -0.06891},
// 								{3.4115e-12, -7.2121e-13, 4.481e-14, -0.010351},
// 								{26.7451, -7.9462, 0.83722, -0.087871},
// 								{-65.1454, 23.0838, -3.4635, -0.6282},
// 								{4.0412, -1.3881, 0.028802, -0.06891}};
float Poly_Coefficient[8][4] = {{3.2723e-13, -9.2113e-14, 7.5235e-15, -0.010351},
								{22.6906, -7.1682, 0.79945, -0.089577},
								{-68.1382, 25.213, -3.8293, -0.67448},
								{2.685, -0.98093, 0.027789, -0.14106},
								{4.8837e-13, -1.2437e-13, 9.691e-15, -0.010351},
								{22.6906, -7.1682, 0.79945, -0.089577},
								{-68.1382, 25.213, -3.8293, -0.67448},
								{2.685, -0.98093, 0.027789, -0.14106}};

float LQR_K_calc(float *coe, float len)
{
	return coe[0] * len * len * len + coe[1] * len * len + coe[2] * len + coe[3];
}

float lqr_k[8] = {
	-0.0104f, -0.0624f, -0.7520f, -0.0704f,
	-0.0104f, -0.0624f, -0.7520f, -0.0704f};

typedef struct
{
	double pos;
	double vel;
} Joint_Motor_t;

typedef struct
{
	double pos;	   // rad
	double vel;	   // rad/s
	float wheel_T; // N*m
} Wheel_Motor_t;

typedef struct
{
	Wheel_Motor_t wheel_motor[2];
	Joint_Motor_t joint_motor[2];

	float target_v;
	float v_set; // 期望速度，单位是m/s
	float x_set; // 期望位置，单位是m
	float v;	 // 实际的速度,单位是m/s
	float v_act;
	float x; // 实际的位移，单位是m

	float phi_set;
	float d_phi_set;

	float turn_set; // 期望yaw轴弧度
	float roll_set; // 期望roll轴弧度

	float leg_set; // 期望腿长，单位是m
	float last_leg_set;

	float v_filter; // 滤波后的车体速度，单位是m/s
	float x_filter; // 滤波后的车体位置，单位是m

	float myPithR;
	float myPithGyroR;
	float myRoll;
	float myRollGyroR;

	float total_yaw;

	int start_flag; // 启动标志
	int front_flag;
	int last_front_flag;
	int turn_flag;
	int last_turn_flag;
	int prejump_flag;
	int jump_flag;	  // 跳跃标志
	int recover_flag; // 一种情况下的倒地自起标志
	int leg_flag;
	int autoleg_flag;
	int autoturn_flag;
	int fastturn_flag;
	int movejump_flag;
} chassis_t;

typedef struct
{
	/*左右两腿的公共参数，固定不变*/
	float right_l1;
	float left_l1;

	float left_T1;
	float right_T1;

	float left_F0;
	float right_F0;
	float roll_F0;

	float left_len;
	float right_len;

	// 腿长变化率
	float left_len_dot;
	float right_len_dot;
} vmc_leg_t;

#define TIME_STEP 5
#define max_vel 6.28
#define TIME_step_s 0.005f
float mg = 2.0f; // 0.875f*9.8

float leg_kp = 40.0f; //-发散
float leg_kd = -2.0f; //+发散

float roll_kp = 0.5f; //-发散
float roll_kd = 0.5f; //-会超调

#endif